package com.rockwell.robot2015.test;

import java.util.ArrayList;

import com.rockwell.robot2015.model.GameStage;
import com.rockwell.robot2015.model.IRobotLogger;
import com.rockwell.robot2015.model.Maze;
import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.RobotConfig;
import com.rockwell.robot2015.model.states.RobotState;
import com.rockwell.robot2015.model.states.RobotStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;
import com.rockwell.robot2015.model.states.running.sequence.FindingTreasureSequence;

import junit.framework.TestCase;

public class FindingTreasureSequenceTester extends TestCase {
	ArrayList<String> traceMessage = new ArrayList<String>();

	private IRobotLogger m_logger = new IRobotLogger() {

		@Override
		public void log(final String msg) {
			traceMessage.add(msg);
		}

		@Override
		public void log(final Exception ex) {
			StackTraceElement[] trace = ex.getStackTrace();
			traceMessage.add(trace.toString());
		}

		@Override
		public void setCurrentState(String s) {
			
		}
		
		@Override
		public void onRobotPaused() {
		}

		@Override
		public void onRobotResumed() {
		}

		@Override
		public void onRobotIdle() {
		}
    };
    
    class TestData {
		boolean[] treasurePresent;
		boolean[] treasureDetected;
		boolean[] obstaclePresent;
		boolean[] armUp;
		double[][] positionData;
		double[] treasurePositionData;
		double[] compassData;
	}


	Robot m_robot;
	RobotStateMachine m_mainMachine;
	RobotRunningStateMachine m_runningMachine;
	FindingTreasureSequence m_sequence;
	
	private void prepare(TestData data) {
		DummyRobotCommander commander = new DummyRobotCommander();
		DummyTreasureDetector treasureDetector = new DummyTreasureDetector(data.treasurePositionData, data.treasurePresent, data.treasureDetected);
		DummyObstacleDetector obstacleDetector = new DummyObstacleDetector(data.obstaclePresent, data.treasureDetected);

		//Maze maze = new Maze(30, 5);

		//m_robot = new Robot(15, 15, null, commander, maze, null, treasureDetector, obstacleDetector);
		RobotConfig config = new RobotConfig();
		config.width = 15;
		config.length = 15;
		config.sensors = null;
		config.commander = commander;
		config.predefinedMazeSolver = null;
		config.treasureDetector = treasureDetector;
		config.obstacleDetector = obstacleDetector;
		config.maze1Detector = null;
		config.outerWallDetector = null;
		
		m_robot = new Robot(config, GameStage.Finals);
		m_robot.setLogger(m_logger, 4);
		m_robot.setStepMode(false);
		
		traceMessage.clear();
		
		m_mainMachine = m_robot.stateMachine();
		m_runningMachine = m_mainMachine.runningStateMachine();
	}
	
	@Override
	public void setUp() {
	}
	
	public void testTurnAndDetected() {
	    TestData data = new TestData()
		{{
			treasurePresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
					false, //test case 3
					false, //test case 4
					false, //test case 5
					false, //test case 6
					false, //test case 7
					false, //test case 8
			};
			
			treasureDetected = new boolean[] { 
					false, //test case 1
					false, //test case 2
					false, //test case 3
					false, //test case 4
					false, //test case 5
					false, //test case 6
					false, //test case 7
					true, //test case 8
			};
			obstaclePresent = new boolean[] { 
					false, //test case 1
					false, //test case 2
					false, //test case 3
					true, //test case 4
					false, //test case 5
					false, //test case 6
					false, //test case 7
					false, //test case 8
			};
			
			armUp = new boolean[] { 
					true, //test case 1
					true, //test case 2
					true, //test case 3
					true, //test case 4
					true, //test case 5
					true, //test case 6
					true, //test case 7
					true, //test case 8
			};
			
			/*
			positionData = new double[][] { 
					{0, RobotPositionDetector.LaneCenterDistance, RobotPositionDetector.LaneCenterDistance}, //test case 1
					{0, RobotPositionDetector.TooCloseToSideWallDistance, RobotPositionDetector.LaneCenterDistance + 2}, //test case 2
					{0, RobotPositionDetector.CloseToSideWallDistance, RobotPositionDetector.LaneCenterDistance + 1}, //test case 3
					{0, RobotPositionDetector.LaneCenterDistance, RobotPositionDetector.LaneCenterDistance}, //test case 4
					{0, RobotPositionDetector.LaneCenterDistance + 2, RobotPositionDetector.TooCloseToSideWallDistance}, //test case 5
					{0, RobotPositionDetector.LaneCenterDistance + 1, RobotPositionDetector.CloseToSideWallDistance}, //test case 6
					{0, RobotPositionDetector.LaneCenterDistance, RobotPositionDetector.LaneCenterDistance}, //test case 7
					{0, RobotPositionDetector.LaneCenterDistance, RobotPositionDetector.LaneCenterDistance}, //test case 8
			};*/
		}};

		String[] expectedMessage = { 	
				"Send Jog Done @ performJog",
				"Wait @ performJog",
				"AmountToTurn[-5], Adjust[-5] @ performAdjust",
				"Stop Done @ performAdjust",
				"Adjust Done @ performAdjust",
				"Jog Done @ performAdjust",
				"Send Jog Done @ performJog",
				"Wait @ performJog",
				"AmountToTurn[5], Adjust[0] @ performAdjust",
				"Stop Done @ performAdjust",
				"Adjust Done @ performAdjust",
				"Jog Done @ performAdjust",
				"Send Jog Done @ performJog",
				"AmountToTurn[5], Adjust[5] @ performAdjust",
				"Stop Done @ performAdjust",
				"Adjust Done @ performAdjust",
				"Jog Done @ performAdjust",
				"Send Jog Done @ performJog",
				"Wait @ performJog",
				"AmountToTurn[-5], Adjust[0] @ performAdjust",
				"Stop Done @ performAdjust",
				"Adjust Done @ performAdjust",
				"Jog Done @ performAdjust",
				"Send Jog Done @ performJog",
				"Wait @ performJog"
				};
		
		RobotAction[] expectedActions = {
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog, 
				RobotAction.Stop, 
				RobotAction.TurnDegree, 
				RobotAction.Jog, 
				RobotAction.Jog
		};

		prepare(data);
		
		/*
		 * Don't call any methods of the Robot class
		 */
		
		m_sequence = m_runningMachine.findingTreasureSequence();
		m_mainMachine.start();
		
		m_mainMachine.move();
		assertFalse(m_sequence.succeeded());
		
		m_mainMachine.move();
		assertFalse(m_sequence.succeeded());
		
		m_mainMachine.move();
		assertFalse(m_sequence.succeeded());
		
		m_mainMachine.move();
		assertFalse(m_sequence.succeeded());
		
		m_mainMachine.move();
		assertFalse(m_sequence.succeeded());
		
		m_mainMachine.move();
		assertFalse(m_sequence.succeeded());
		
		m_mainMachine.move();
		assertFalse(m_sequence.succeeded());
		
		m_mainMachine.move();
		assertTrue(m_sequence.succeeded());
	}
}
